#include "Main.h"

extern std::list<Triangle> listTriangle;

Collision::Collision(){

}

Collision::~Collision(){
	
}

bool Collision::CollideEllipsoid(Vector &Center, Vector &Direction){
	return EllipsoidIntersectScene(Center,Direction);
}

bool Collision::EllipsoidIntersectScene(Vector &Center,Vector &Direction){
	std::list<Triangle>::iterator it;

	Vector VelocityVector;
	VelocityVector.x = -Direction.x;
	VelocityVector.y = -Direction.y;
	VelocityVector.z = -Direction.z;

	/*
	float t;
	Vector intersectionPoint;
	for(it=listTriangle.begin();it!=listTriangle.end();it++){
		if(Collision::RayIntersectPlane(*it, &campos, &(VectorAddition(&VelocityVector,&campos)), &t,intersectionPoint)){
			if(Collision::PointInTriangle(intersectionPoint,*it)){
				return true;
			}
		}
	}*/
	float eInterval = 1.0f;
	float D;
	Vector RemainingVelocity;
	Vector CollisionNormal;
	for(it=listTriangle.begin();it!=listTriangle.end();it++){
		if(SphereIntersectTriangle(Center, 1.0f,VelocityVector, *it, eInterval, CollisionNormal)){
			RemainingVelocity = VectorMultiplication(&VelocityVector,1.0f-eInterval);
			D = Matrix3DotProduct(&RemainingVelocity,&CollisionNormal);
			Direction = VectorSubtraction(&(VectorMultiplication(&CollisionNormal,D)),&VelocityVector);
			return true;
		}
	}

	return false;
}

bool Collision::SphereIntersectPlane(Vector &planeNormal,Vector &v1,Vector &Center,Vector &Velocity,float Radius,float &tMax){
	float t;
	float PlaneDistance = Matrix3DotProduct(&(VectorSubtraction(&Center,&v1)),&planeNormal) - Radius;
	float ProjRayLength = Matrix3DotProduct(&Velocity, &planeNormal);
 	if(PlaneDistance < 0.0f || ProjRayLength >-0.0000001f){
		if(ProjRayLength > -1e-5f) return false;
		if(PlaneDistance < -(Radius)) return false;
		tMax = ProjRayLength;
		return true;
	}
	//if(abs(ProjRayLength) < 1e-5f) return false;
	t = -(PlaneDistance/ProjRayLength);
	if(t < 0.0f || t > tMax) return false;
	//IntersectionPoint = VectorAddition(VectorMultiplication(VectorSubtraction(Direction,Origin),*t),Origin);
	//IntersectionPoint = VectorAddition(&(VectorMultiplication(&(VectorSubtraction(Velocity,Center)),t)),Center);
	tMax = t;
	return true;
}

bool Collision::RayIntersectPlane(Triangle &testTriangle,Vector *Origin,Vector *Direction,float *t, Vector &IntersectionPoint){
	Plane testPlane;
	testPlane.a = testTriangle.v1.y*(testTriangle.v2.z - testTriangle.v3.z) + testTriangle.v2.y*(testTriangle.v3.z - testTriangle.v1.z) + testTriangle.v3.y*(testTriangle.v1.z - testTriangle.v2.z);
	testPlane.b = testTriangle.v1.z*(testTriangle.v2.x - testTriangle.v3.x) + testTriangle.v2.z*(testTriangle.v3.x - testTriangle.v1.x) + testTriangle.v3.z*(testTriangle.v1.x - testTriangle.v2.x);
	testPlane.c = testTriangle.v1.x*(testTriangle.v2.y - testTriangle.v3.y) + testTriangle.v2.x*(testTriangle.v3.y - testTriangle.v1.y) + testTriangle.v3.x*(testTriangle.v1.y - testTriangle.v2.y);
	testPlane.d = testTriangle.v1.x*(testTriangle.v2.y*testTriangle.v3.z-testTriangle.v3.y*testTriangle.v2.z)+testTriangle.v2.x*(testTriangle.v3.y*testTriangle.v1.z-testTriangle.v1.y*testTriangle.v3.z)+testTriangle.v3.x*(testTriangle.v1.y*testTriangle.v2.z-testTriangle.v2.y*testTriangle.v1.z);
	Vector planeNormal;
	planeNormal.x = testPlane.a;
	planeNormal.y = testPlane.b;
	planeNormal.z = testPlane.c;
	Normalize(&planeNormal,&planeNormal);
	float PlaneDistance = Matrix3DotProduct(&(VectorSubtraction(Origin,&(testTriangle.v1))),&planeNormal);
	float ProjRayLength = Matrix3DotProduct(&(VectorSubtraction(Direction,Origin)), &(VectorNegate(&planeNormal)));
	if(abs(ProjRayLength) < 1e-5f) return false;
	*t = PlaneDistance/ProjRayLength;
	if(*t < 0.0f || *t > 1.0f) return false;
	//IntersectionPoint = VectorAddition(VectorMultiplication(VectorSubtraction(Direction,Origin),*t),Origin);
	IntersectionPoint = VectorAddition(&(VectorMultiplication(&(VectorSubtraction(Direction,Origin)),*t)),Origin);
	return true;
}

bool Collision::PointInTriangle(Vector point, Triangle &triangle){
	Vector edge, direction, edgeNormal;
	//First Edge
	edge = VectorSubtraction(&(triangle.v2), &(triangle.v1));
	direction = VectorSubtraction(&(triangle.v1), &point);
	edgeNormal = Matrix3CrossProduct(&edge, &(VectorComputeNormal(&(triangle.v1),&(triangle.v2),&(triangle.v3))));
	Normalize(&edgeNormal, &edgeNormal);
	if(Matrix3DotProduct(&direction, &edgeNormal)<0.0f) return false;
	//Second Edge
	edge = VectorSubtraction(&(triangle.v3), &(triangle.v2));
	direction = VectorSubtraction(&(triangle.v2), &point);
	edgeNormal = Matrix3CrossProduct(&edge, &(VectorComputeNormal(&(triangle.v1),&(triangle.v2),&(triangle.v3))));
	Normalize(&edgeNormal, &edgeNormal);
	if(Matrix3DotProduct(&direction, &edgeNormal)<0.0f) return false;
	//Third Edge
	edge = VectorSubtraction(&(triangle.v1), &(triangle.v3));
	direction = VectorSubtraction(&(triangle.v3), &point);
	edgeNormal = Matrix3CrossProduct(&edge, &(VectorComputeNormal(&(triangle.v1),&(triangle.v2),&(triangle.v3))));
	Normalize(&edgeNormal, &edgeNormal);
	if(Matrix3DotProduct(&direction, &edgeNormal)<0.0f) return false;

	return true;
}


bool Collision::SphereIntersectLineSegment(Vector &Center, float Radius, Vector &Velocity, Vector &v1, Vector &v2, float &tMax, Vector &CollisionNormal){
	Vector E,D,x,y,PointOnEdge,CollisionCenter;
	float a,b,c,d,e,t,n;
	float L;
	E = VectorSubtraction(&v2, &v1);
	D = VectorSubtraction(&Center, &v1);

	e = VectorLength(E);
	if(e<1e-5f) return false;
	E = VectorMultiplication(&E, 1.0f/e);

	

	x = Matrix3CrossProduct(&D, &E);
	y = Matrix3CrossProduct(&Velocity, &E);
	a= VectorLength(y) * VectorLength(y);
	b= 2.0f * (Matrix3DotProduct(&y,&x));
	c= (VectorLength(x) * VectorLength(x))-(Radius*Radius);
	
	//sphere center is already inside cylinder
	if(c>=Radius) return false;
	if(c<0.0f){
		d = Matrix3DotProduct(&D, &E);
		if(d < 0.0f){

			//TODO:return sphereintersectpoint
			return false;
		}else if(d > e){

			//TODO:return sphereintersectpoint
			return false;
		}else{
			PointOnEdge = VectorAddition(&v1,&(VectorMultiplication(&E,d)));
			CollisionNormal = VectorSubtraction(&Center,&PointOnEdge);
			n = VectorLength(CollisionNormal);
			CollisionNormal = VectorMultiplication(&CollisionNormal, 1.0f/n);
			t = n-Radius;
			if(tMax < t) return false;
			tMax = t;
			
			return true;
		}
	}
	if(tMax < 0.0f) return false;
	if(!SolveCollision(a,b,c,t)) return false;
	if(t > tMax || t==0.0f) return false;

	CollisionCenter = VectorAddition(&Center, &(VectorMultiplication(&Velocity,t)));
	d = Matrix3DotProduct(&(VectorSubtraction(&CollisionCenter,&v1)),&E);
	if(d<0.0f)
		return false;
	else if (d>e)
		return false;
	PointOnEdge = VectorAddition(&v1, &(VectorMultiplication(&E,d)));
	CollisionNormal = VectorSubtraction(&CollisionCenter,&PointOnEdge);
	Normalize(&CollisionNormal, &CollisionNormal);
	tMax = t;
	return true;
}


bool Collision::SolveCollision(float a, float b, float c, float &t){
	float d, one_over_two_a, t0,t1,temp;

	d = b*b - 4*a*c;

	if(d<0.0f) return false;

	d = sqrtf(d);
	one_over_two_a = 1.0f / (2.0f * a);

	t0 = (-b - d) * one_over_two_a;
	t1 = (-b + d) * one_over_two_a;

	if(t1 < t0){ temp = t0; t0=t1; t1 = temp;}

	if(t1 < 0.0f) return false;

	if(t0 < 0.0f) 
		t = t1;
	else 
		t = t0;
	if(t=0.0f) return false;
	return true;
}

bool Collision::SphereIntersectTriangle(Vector &Center, float Radius, Vector &Velocity,Triangle &CollTriangle, float &tMax, Vector &CollisionNormal){
	float t = tMax;
	Vector CollisionCenter;
	bool bCollided = false;
	Plane testPlane;
	testPlane.a = CollTriangle.v1.y*(CollTriangle.v2.z - CollTriangle.v3.z) + CollTriangle.v2.y*(CollTriangle.v3.z - CollTriangle.v1.z) + CollTriangle.v3.y*(CollTriangle.v1.z - CollTriangle.v2.z);
	testPlane.b = CollTriangle.v1.z*(CollTriangle.v2.x - CollTriangle.v3.x) + CollTriangle.v2.z*(CollTriangle.v3.x - CollTriangle.v1.x) + CollTriangle.v3.z*(CollTriangle.v1.x - CollTriangle.v2.x);
	testPlane.c = CollTriangle.v1.x*(CollTriangle.v2.y - CollTriangle.v3.y) + CollTriangle.v2.x*(CollTriangle.v3.y - CollTriangle.v1.y) + CollTriangle.v3.x*(CollTriangle.v1.y - CollTriangle.v2.y);
	testPlane.d = CollTriangle.v1.x*(CollTriangle.v2.y*CollTriangle.v3.z-CollTriangle.v3.y*CollTriangle.v2.z)+CollTriangle.v2.x*(CollTriangle.v3.y*CollTriangle.v1.z-CollTriangle.v1.y*CollTriangle.v3.z)+CollTriangle.v3.x*(CollTriangle.v1.y*CollTriangle.v2.z-CollTriangle.v2.y*CollTriangle.v1.z);
	Vector triNormal;
	triNormal.x = testPlane.a;
	triNormal.y = testPlane.b;
	triNormal.z = testPlane.c;
	Normalize(&triNormal,&triNormal);
	//sphere intersect plane
	if(!SphereIntersectPlane(triNormal,CollTriangle.v1, Center, Velocity, Radius, t)) return false;
	//point in triangle
	if(t<0){
		CollisionCenter = VectorAddition(&Center, &(VectorMultiplication(&triNormal, -t)));
	}
	else{
		CollisionCenter = VectorAddition(&Center, &(VectorMultiplication(&Velocity,t)));
	}
	if(PointInTriangle(CollisionCenter,CollTriangle)){
		CollisionNormal = triNormal;
		tMax = t;
		return true;
	}

	bCollided |= SphereIntersectLineSegment(Center,Radius,VectorSubtraction(&Center,&Velocity),CollTriangle.v1,CollTriangle.v2,tMax,CollisionNormal);
	bCollided |= SphereIntersectLineSegment(Center,Radius,VectorSubtraction(&Center,&Velocity),CollTriangle.v2,CollTriangle.v3,tMax,CollisionNormal);
	bCollided |= SphereIntersectLineSegment(Center,Radius,VectorSubtraction(&Center,&Velocity),CollTriangle.v3,CollTriangle.v1,tMax,CollisionNormal);

	return bCollided;
}